#define STATE_SHAKING 1
#define STATE_STOPPED 2

#define PIN_STEPPING 7
#define PIN_ENABLE 9
#define PIN_LED 13


// serial read buffer
byte readBuffer[16];
int readBufferIndex = 0;

int state = 0;

void setup()         
{
  Serial.begin(9600);

  pinMode(PIN_STEPPING, OUTPUT);
  pinMode(PIN_ENABLE, OUTPUT);
  pinMode(PIN_LED, OUTPUT);
  digitalWrite(PIN_ENABLE, LOW);
  digitalWrite(PIN_STEPPING, LOW);
}

void loop()                       
{
  // Read incomming a package
  readBufferIndex = 0;
  while(Serial.available() > 0)
  {
    // Delay between reads
    delay(20);
   
    readBuffer[readBufferIndex] = Serial.read();
    readBufferIndex ++; 
    
    // If package size have been read we can test for package end
    if(readBufferIndex > 0) {
      int packageSize = readBuffer[0];
      if(readBufferIndex == packageSize) {
        break;
      }  
    }
  }

  if(readBufferIndex > 0) {

    // Delay before write
    delay(20);

    handleIncommingPackage();  
  }  
  
  if(state == STATE_SHAKING) 
  {
    shakeUpdate();
  }  
}

void handleIncommingPackage() 
{
  int packageSize = readBuffer[0];
  int data = readBuffer[1];
  
//  String out = "handle " + data;
//  Serial.print(out);
  switch(data)
  {
    case 1: 
      startShake();
      break;
    case 2: 
      stopShake();
      break;
  }
}

void startShake() {
  
  delay(1000);  
  //digitalWrite(PIN_LED, HIGH);   
  
  state = STATE_SHAKING;
  
  // Send SHAKING when shaking has started
  Serial.write(2); // size
  Serial.write(STATE_SHAKING); // msg
}

void stopShake() {
  
  delay(1000);  
  //digitalWrite(PIN_LED, LOW);   
  
  state = STATE_STOPPED;
  
  // TODO: make shaker stop at specific position
  
  // send STOPPEND when shaking actually has stopped
  Serial.write(2);
  Serial.write(STATE_STOPPED);
}

void shakeUpdate()
{
  digitalWrite(PIN_STEPPING, HIGH); // 16hz signal til control chip. den giver 1 puls
  delay(1);
  digitalWrite(PIN_STEPPING, LOW);
  digitalWrite(PIN_LED, HIGH);
  delay(100);                  // denne pause forstår stepper motor.

  digitalWrite(PIN_STEPPING, HIGH); // 16hz signal til control chip. den giver 1 puls
  delay(1);
  digitalWrite(PIN_STEPPING, LOW);
  digitalWrite(PIN_LED, LOW);
  delay(100); 
}
